// Have include guard.
#ifndef TESTROBOT_H
#define TESTROBOT_H

#include "WPILib.h"
#include "EntechFpk.hpp"
#include "TestDrive.hpp"
#include "PeriodicPrinterSubsystem.hpp"
#include "TestAutonomousController.hpp"
#include "DriveDistanceTestController.hpp"

/**
 * A test robot.
 * For developers, serves as a test to see if everything works like it should
 * and to see if the system is easy to implement.
 */
class TestRobot :
	public SystematicRobot
{
	public:
		TestDrive m_drive;
		
		TestRobot();
		
		virtual void Initialize();
	private:
		template<typename DriveController>
		static void SetupDriveController(DriveController &driveController)
		{
			driveController.GetModifiers().push_back(new MecanumDriveSubsystem::DeadZoneModifier(0.15f));
			//driveController.GetModifiers().push_back(new MecanumDriveSubsystem::ScaleModifier(0.5f));
		}
		
		PeriodicPrinterSubsystem m_periodic;
		
		Joystick m_joystickLeft;
		Joystick m_joystickRight;
		XboxJoystickInput m_joystick;
		PS3JoystickInput m_ps3Joystick;
		GenericHidDigitalInput m_digitalInputDriveInputUp;
		GenericHidDigitalInput m_digitalInputDriveInputDown;
		GenericHidDigitalInput m_digitalInputDriveModeTwoJoysticksUp;
		GenericHidDigitalInput m_digitalInputDriveModeTwoJoysticksDown;
		GenericHidDigitalInput m_digitalInputDriveModeXboxUp;
		GenericHidDigitalInput m_digitalInputDriveModeXboxDown;
		GenericHidDigitalInput m_digitalInputDriveModePs3Up;
		GenericHidDigitalInput m_digitalInputDriveModePs3Down;
		SmartDashboardStringOutput m_outputDriveMode;
		
		//TestAutonomousController m_controllerAutonomous;
		DummyController m_controllerAutonomous;
		DriveDistanceTestController m_controllerTestDriveDistance;
		CompoundController m_controllerTeleop;
		ControllerSelectorController m_controllerDriveJoystickSelector;
		ControllerSelectorController m_controllerDriveSelectorTwoJoysticks;
		ControllerSelectorController m_controllerDriveSelectorXbox;
		ControllerSelectorController m_controllerDriveSelectorPs3;
		TankDriveWithPassiveMecanumController m_controllerDriveTankPassiveMecanum;
		TankDriveWithActiveMecanumController m_controllerDriveTankActiveMecanum;
		ArcadeDriveNormalStyleWithMecanumController m_controllerDriveArcadeNormalMecanum;
		ArcadeDriveModernStyleWithMecanumController m_controllerDriveArcadeModernMecanum;
		XboxArcadeDriveModernStyleWithMecanumController m_xboxArcadeDriveModernStyleWithMecanumController;
		XboxTankDriveWithPassiveMecanumController m_xboxTankDriveWithPassiveMecanumController;
		XboxArcadeDriveNormalStyleWithMecanumController m_xboxArcadeDriveNormalStyleWithMecanumController;
		PS3TankDriveWithPassiveMecanumController m_ps3TankDriveWithPassiveMecanumController;
		PS3ArcadeDriveModernStyleWithMecanumController m_ps3ArcadeDriveModernStyleWithMecanumController;
		PS3ArcadeDriveNormalStyleWithMecanumController m_ps3ArcadeDriveNormalStyleWithMecanumController;
		PS3TiltDriveStyleWithMecanumValueFetcher m_ps3TiltDriveStyleWithMecanumController;
		
		
};

#endif // #ifndef TESTROBOT_H
